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ABSTRACT 

A combination of B~£^line representation and the inethcK 
of local variations CMLVO has been used for off-line optima!: 
pi anni rig of robot Joint tra Jector i es . B-Splines have severa. 
advant ► for robot trajectory r epr esentati on ► in terms at 

accuracy of the representation and its derivatives and in terms o1 
the ease with which computations can be done. Furthermore^ because 
of their property of local support, B-Spli nes are well -suited for 
use with MLV. , Two typ>es of optimal Joint trajectories have beer 
determined: C i Z) trajectories to obtain minimum energy loss in thf 
Joint motors, for fixed tin^e of traversal of the end effectoi 
between two given fxcints in cartesian space and C2Z> trajectoriej 
which minimize the time of traversal of the end effector betweei 
two given f>oints in cartesian space. The trajectories obtaine 
satisfy constraints on p^osition, velocity, acceleration and Jer! 
in the Joint space. The algorithms developed have been used t< 
solve the planning problems posed for PUMA - B60 robo 

manipulator, Hiough obstacle constraints have not been considere 
here, they can be incorporated without difficulty in th 


algorithms developed 
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CHAPTER 1 

INTRODUCTION 

The need for flexibility^ improved product! vi ty* better quality 
and lower production cost in industries has motivated the speedy 
growth of automation in manuf acturi ng processes. :^:3€fcial purpose 
machines were the first step towards this automation. Ctevelopment 
of HC and CMC machine tools was the next major event in 
autoamation in manuf acturi ng processes. The developments which 
are now taking place in Robotics will ultimately usher in an era 
of automatic factories. 

An industrial robot is a mul tifunctional 
computer -control led unechanical manipulator capable of performing a 
variety of tasks through motions along several directions. Robot 
manipulators need to be controlled in efficient and effective ways 
to enable them to perform the multitude of complex tasks for which 
they are designed. So» Robot control has evolved as an important R 
& D area for control engineers. 

11 Trajectory Planning And Motion Control 

E^ch of the robot Joints is provided with its own 
control loop which is actually a servomechanism. Actuators 
driving the robot Joints are generally dc servomotors. So^ the 
control of robot manipulator is actually the control of Joint ; 
actuators to make the end effector CEEZ) ii»ve efficiently along a 
prespecified path. Efficiency here implies mainly two types of ; 
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control, namefly, eont,rc>l Tor minimum energy and minimum 'time 
operation. In the Tormer type of control , robot manipulators are 
operated in such a way that the energy required to move the EE 
from an initial position to the target position is minimum and in 
the latter typje the EE moves from the initial to the final 
location in minimum time. The basic problem here is to determine 
the appropriate control signals to the Joint servomechanisms. 

Because of the nonlinearity and coupling in robot 
dynamics, the control problem of robot manipulator is much 
Involved and requires large computational effort for its solution. 
Usually trajectory control is done in two step>s : Off-line optimal 
trajectory planning and On-line tracking. A schematic diagram of 
robot trajectory control is shown in Fig. 1.1 . 

First of all, the desired task must be SF»eclfied to 
Task Planner, that is, the EE position and orientation, both 
initial and final, must be specified in cartesian coordinates. 
Secondaly, Task Planner must have a complete knowledge of 
obstacles in cartesian coordinates. An obstacle may be static or 
dynamic. If it is dynamic, its p>osition as a function of time must 
be known apriori. Task Planner generates an ordered sequence of 
points in cartesian space which results in a collision-free path. 

Ctff-llne optimal path plannig is usually done in Joint 
space because of the direct involvement of controlled variables, 
fast confutation time and ease of handling dynamic constraints on 
torques or forces. Also, it is simpler than cartesian space 
planning. So, Geometric Path Planner first uses the inverse 
kinematic algorithm CIKAJ to find the values of Joint variables at 
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Fig 1 , 1 - A schematic diagram o£ robot trajectory control 
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knot points corresponding to the cartesian knot sequence, 
Cartesian knot sequence is the sequence of points at which EE 
positions and orientations are specified. Now^ having 

speci fiction of Joint variables at knot points » continuous Joint 
trajectories in parameterized form are obtained by a suitable 
i nterpol ati on al gor i thm. 

Interpolator output is fed to Trajectory Planner- 
Path constraints are given as input to Trajectory Planner. Path 
constraints are specified in terms of maximum reach C position 
constraints!) » maximum velocity^ maximum acceleration^ etc. Also 
p^ath constraints sp>ecify some performance criterion to be 
mini mi zed » C usually time!>» in traversing the EE along a particular 
path. Here comes the optimization problem into picture^ that is» 
the Joint trajectoris should be optimized for some criterion 
function together with satisfaction of all the constraints 
C boundary constraints and path constraints !) . Dynamic constraints 
such as constraints on actuator torque^orce etc are also 
sppecified and taken into account by the trajectory planner. Thus 
the output of the trajectory planner is the optimal time sequence 
of position* velocity* acceleration* etc. Here* it should be 
noted that the optimization algorithm does not change the 
geometric path* 1 . e. * the EE moves along the path decided by the 
task planner. Only the velocities and accelerations of varlot^s 
Joints are determined at various time instants to optimize the 
f>erf ormance criterion. 

It is clear that the geometric path itself can be 
modified during optimization. This can substantially improve 




Fig. 
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robot performamcffif in comparison with that obtained! by a 
non-optimized geometric path* C7J. For this we can have a scheme 
where a preliminary geometric path planning and trajectory 
planning exercise can be done to generate not only a geometric 
path but also a feasible non optimal trajectory with respect to 
tiis«r in cartesian space according to some rule. This cartesian 
trajectory is then converted into Joint spxaice trajectories. The 
main trajectory planner takes obstacles into account and uses the 
Direct Kinematic Algorithm CDKA:> to find the positions of the 
different Joints and links in cartesian space in order to cheek 
the obstacle presence. It uses a suitable optimization algorithm 
to obtain an optimal robot trajectory specified in terms of Joint 
space variables. We can further consider that this trajectory 
planner also executes the initial task of generating a feasible 
trajectory. In this Trajectory Planner* the traditional 

distinction between Geometric Path Planner and Trajectory Planner 
is no longer present CFig. 1 . £} . 

The output of the trajectory planner is fed to the 
Trajectory Tracker* where the actual manipulator movement is 
compared with the planned one and* according to the error 
encountered* corrective measures are taken by the appropriate 
controllers. 

12 Review Of The Earlier^ Works 

Various contributions have been made in the past 
decade to the area of optimal path planning. A brief review of 
some of them is given below. 
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L\ih and Lin 133 have solved the Minimum Time 
Trajectory Planning CMTIP!) problem by using nonlinear programming. 
The fiath to be optimized was defined by a set of corner points 
connected through straight lines with circular arcs in the 
neighbourhood of corner p»olnts. Constant or piecewise constant 
bounds with respect to time were assumc^d on robot velocity and 
accel er atl on. These bounds depend on p>osition» p»ayload mass and 

its shape etc. Thus, for efficient and safe working of this 
scheme, the upper bounds were necessarily chosen as the global 
greatest lower bounds of velocity and acceleration values, that 
is, the worst case limits were used. This Implies underutilization 
of robot capabilities and may result in considerable inefficiency. 
For example, robot may take too much time to perform a particular 
task . 

Shin and He Kay [43 and Bobr-ow [53 attempted solving 
MTTP problem using phase plane technique. Robot dynamics was 
described using parametric function in Joint sp>ace. Geometric 
path constraints and dynamic constraints were taken into account. 
Both the methods assume that there is a set of admissible 
velocities for a given pjosltion which leads to the concept of an 
admissible region in the phase plane. The solution to the 
optimization problem has been obtained by a switching curve in the 
phase plane. The method of [43 is applicable to the general case 
of noh-simply connected admissible regions while [53 works on the 
assumption of simply-connected admissible regions. 

Kin and Shin [G3 presented a solution to MTTP problem 
in Joint spvace. The p>ath was assumed to be straight line between 
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corner points. The solution to the global HTTP problem was 
obtained through local optimization. For this, the actual problem 
was replaced by a set of local optimization problems, one at every 
local corner point. This method is not applicable for the case 
v^ere a path segment is too short to include the cruise stage. 

Shin and McKct^ 171 have again taken up the MTTP 
problem. In this work, first a lower bound was derived on the 
time required for a particular manipulator movement and then this 
bound has h>een minimized. The paper, thus has developed a method 
for finding a geometric path for robot motion from a given initial 
point to a given final point in minimum time. Parameterization 
approach has been used to describe the geometric path. 

Su?-L and Shin [8J presented a near optimal path for the 
EE movement using variational calculus and dynamic programming 
methods. The cost function was defined as a weighted distance 
function BO that obstacles could be avoided and the total distance 
of travel minimized simultaneously. 

A totally discrete approach has h>een adopted by Tctn 
and Potts t©] for the solution of MTTP problem. This uses the 
method of approximate programming CMAP2>, which is a sequence of 
linear subprograms, to solve this problem. 

A technique for solving MTTP with obstacle constraints 
has been presented by Bobx-ota [103 through the use of a distance 
function. A B-S^line polynomial representation has been used for 
the cartesian path of the manipulator. 

The method of local variations CMLVZ) has been used by 
Patrihar- C113 and Soshadr-i £1S3 to solve Minimum Energy Trajectory 
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Planning CMETP^ and HTTP probla-ms. fl23 has tak^an obstael#ps» both 
static and d 3 mamic» into consi derati on. Finite difference method 
has been used for the approximation of Joint trajectories in both 
the cases. 

Ltn and Lxjih Cl 33 and Pot el have solved optimal path 
planning problem using B-Splines. The optimization criterion is 
minimum time in both the cases. Cl 33 uses nonlinear programming 
to solve this problem while Ci4.3 uses linear scaling. They have 
not taken dynamic constraints into consi der ati on. 

13 Objective Of Tic Present Work 

In this thesis two problems of off-line optimal 
trajefctory planning have been discussed* namely* minimum energy 
and minimum time trajectory planning. The objective of the first 
problem is to find an optimal EE path to move the EE from an 
initial specified position to the given target position in 
specified time* so that the electrical energy consumption by the 
robot drive motors is minimized. In the second problem an optimal 
path is found for the EE to go from a given initial position to a 
given final position in minimum time. 

For the sake of simplicity trajectory planning has 
been done in a col 1 i si on free envi r onment . However * t hi s method 
can take account of obstacles with equal efficiency. In both the 
problems B-S^lines have been used for interpolation of Joint 
trajectories. 

The algorithm used in this thesis for optimization of 
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Ihe spline i nlerpol aLed Joint, t,raject/OrieS|, cJilled the Method of* 
Local Variations CML\0* was originally developed by Cher-noiis^ Xo. 
It is basically a method to obtain the numerical solution oJT fixed 
time optimal control pr obi ems* by systematic local perturbations 
of the state trajectory. This method can take account of 

magnitude constraints on state variables easily. 

The main contribution of the work reposed here lies in 
the use of a combination of B-S^lines and MLV. As MLV works on 
the concept of systematic local perturbations of a feasible 
trajectory to find an optimal one» B-Spline representation of 
trajectory makes it easy to carry out local pxertur bat ions since 
B-Splines have the property of limited support, So^ if a spline 
interpolated trajectory is pn&rturbed by perturbing a spline 
coefficient* only a small portion of the whole trajectory gets 
perturbed* not the whole trajectory. Secondly* these splines 
enable us to easily incorporate derivative continui ties. Thus* we 
can have continuous position* velocity acceleration and jerk if 
fifth order splines are used. This is a very desirable feature for 
any trajectory planning scheme. 

14- Organization Of The Thesis 

Chapter 2 presents piecewise polynomial approximation 
and gives a brief introduction to B-S^lines. 

Chapter 3 presents an interpolation scheme using 
B-S^lines. It also describes the essential features of the method 


of local variations CML\0. 


Pseudocodes for the interpolation 
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cch«rm* and optimization using MLV in combination with B-J^linn&s 
ara- giva-n. 

Chapter 4 considers the METIP problem using both cubic 
and quart! c B-Splines. Results of computer simulation of the 
first three Joints of PUMA SGO robot are given. 

Chapter 5 gives a solution to the MTTP problem. An 
algorithm using MLV and B-S^lines is given. Computer simulation 
results are presented. 

Conclusions of the present study are presented and 
discussed in chapter 6. 2Some suggestions for future work in this 
area are also given. 



CHAPTER 2 


AN INTRODUCTION TO B-SPLINES 

Given a. function fCxI) over an interval [a>b], we say that 
another function is an approximation to it, if yfCyO is close 

to fCx!> in some sense. Closeness of fCxZ> and v<xZ> can, for 

example, be Judged in terms of a suitable norm. Thus, if fCx!) and 
y<IxD are members of a normed linear space, then the closeness of 
the functions can be Judged in terms of the norm of fCxD - ^xD , 
represented by the notation | f — ^ |. For these functions to be 
close, the above norm should be small. One way of obtaining an 
approximation to a function fCxiS over an interval [a,b3 is to 
utilize values fCx^I) of the function at a discrete set of points, 

* i =1 ,2, . . . ,n. If the approximation ^xD is 

such that fCx^:> = v<x^5 , i = 1 , 3, . . . , n , then we say that 

^xZ> is an interpellating approximation to fCxJ or sinqply as 

interpolating function for the data fCx^:), i = 1 , 2, . . . ,n. 

Polynomials are widely used as lnterp»olating 
functions . The main advantage in their use is the ease with which 
they can be evaluated , differentiated and integrated with the aid 
of simple arithmetic op>erations . A polynomial of order n can k>e 
represented as 

e>CxD = a_ + a, X + . . . + a x"^ ^ C2. 1!) 

O i n 

We will only consider real valued functions of real variables. 
Let represent the set of all polynomials of order n. Then with 
the usual rules of addition and scalar multiplication, this set 
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forms a. linear space. 

Some di sad van-tages with polynomial interpolation are the 
f ol 1 owi ng : 

In order to have good accuracy* it is often necessary to have 
high degree f>ol ynomi al s and to determine the large number of 
coefficients involved we have to use sufficiently large number of 
data points. There is usually large computational effort,and 
numerical problems are encountered in the determination of the 
coef f i ci ents . 

Polynomials yield good approximation if interpol ati on 
points are chosen appr cor lately and the function to be 
approximated £>ehaves in a uniform manner throughout the interval 
of approximation. In fact* the polynomial interpol ant is very 
sensitive to the choice of interpolation points £13. Furthermore * 
if the behaviour of the function to be approximated differs 
significantly over a subinterval of the interval of approximation 
from its behaviour elsewhere* C for example rapid oscillations 
over a subinterval D* then the approximation is poor everywhere 
£13. This global dependence of approximation over local 

properties of the function to be approximated makes polyriomial 
approximation unattractive in many situations. Piecewise 

polynomial approximations are much better suited for these 
situations. B-'S^line approximations are a special form of 
piecewise polynomial approximations. These approximations yield 
better accuracy and have advantages in numerical computations. We 
will develop the concept of these approximations below. 
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2.1 Recewise polynomial Cpp) function 

Let < Y)^ > , i = 1 , a, . . m +1 be & strictly increasing 

sequence of that is, let y)^ satisfy the condition ol = 

< ff. <...<17 <17 ^ =b. Let K be a p>ositive integer. *<IxZ) is 

-to a pp Turic-tion of" ordar 1C C IC-i^ * ov«pr t/har 

inl«prval £a»b3» if" it is a polynomial or order 1C C degree < K-l> in 

each of the intervals ^ >7^ » ^ ^ ^ J2» . . . »m. 

2.2 Spline functions 

Actually spline is a mechanical device used by draftsmen 
for drawing a smooth curve. It consists of a strip of rod of some 

flexible material which can be bent to constrain it to 

approximately pass through certain plott<Kl points on a graph. The 
term spline function implies that the graph of such a funtion is 
similar to a curve dravm by a mechanical spline. Saline functions 
are a class of pp functions satisfying the additional property of 
derivative continuity, as explained below 

Dc/irLition - A spline function sCx 2 > of order K is defined to be 

continuous pp of order K if following two conditions are satisfied 

ay it is in C[a,b} <ri.e, is a continuous function of x over 

the interval a x :£ bZl , and there exists a strictly increasing 

sequence of real p>oints a = 17^ < 17^ < ... < 17 = b such that 

»Cx 3 isj A polynomiAl of degree at most K-1 on e?Ach of thie 

i nter val ^ ^ = 2 » 3 * , . . » m+1 . 

K— 2 

City it is also in lh<ip sp&CA C Ca»b 3 » » it has continuous 
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deriv&iiv«fS up to CIC-SZ) th order over the Interval & £ k < h. The 
points ► i = are called knot points or simply as 

knots. 

2.3 LtNtAR Space Of Splwe Fi^ictiohs 

ir s^CxZ) and s^dxD are spline functions of order 1C over 
£a»b] defined with resp>ect to the knot sequence ^ i = 1 » . 

then o s^CxZ? s^CxZ? is also a spline function of 

order 1C with respect to the same knots » where oi and ft skr^ any two 
scalars. Thus we can speak of the linear space of spline 
functions of order 1C over [a^bl with respect to the knots » i 
= 1 > . . » m^-l . 

Now * if we consider pp functions defined over m +1 knots 

T >1 . over each subinterval ^ 4 . j; ^ PP 

function is represented in terms of a ICth order polynomial 

C character! zed in terms of 1C parameters!) » then^ in all we require 

ICm parameters for the representation of the function. Thus the 

dimension of such a space of pp functions is Knfi. This space is 

represented by the symbol :P . Now* if we impose continuity 

» 17 

condition to be satisfied at the knots* the number of free 
piarameters to define the function is is ICm - Cm-’ID. If the pp 
function is to have continuous derivatives up to order lC- 2 * then 
the number of free parameters required to define the function is 
Km - Cm~i3ClC-13 = m+lC-1 . Thus* the dimension of the space of 
spline functions of order 1C defined with respect to knots y)^*i- 
1*2*,..* m-M is Cm+K-ID. If the pp function is to have 
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continuous derivatives at the knot f>oints up to order ^ IC-S* 

then the number of free parameters^ or ai ter natively » the 
difinension of the space of these functions is ICm *- C m-1 C D , We 
will use the symbol to denote this space. Clearly, 

is a subspace of ^ . In this nc .ation, the space of spline 

K » Y) 

functions is denoted by _ _ . 

» 17 •> 1C““2 

2.4 Selection Of A Basis For Thc Space Of Splines , ^ ,^_2 - 

Computational efficiency for problems employing 

splines requires construction of a suitable basis for the space of 

splines P , that is , we need a sequence ^ ^ , ...» of 

12 

functions » all in _ _ such that every element of P^ ^ _ 

^ K. , Y) > 

can be written as a unique linear combination , E 

j J J 

sequence 4 ^ ^ 4 ^ ^ , . . » . 

2.4.1 The Tri>4cated Power basis . 

Each mc-mber of Lho subspace _ . can be written 

K ► 17 ► K-2 

as 

K-l ^ m 

,a s; X i b C2. 25 

where 

Cx~Y)j>^ s max: CO,. x-»Y]f j 3 

Tlie functions of the type Cx ~)7 D are called truncated functions. 

vf 

The parameters C, J ==0,1, . . . tlC-l and d ,j-2». . . ,m 

■Jf w 

distinguish the different members of P^ ^ Clearly fCx2) is 

^ K:,Y)»1C-2 


tCyO 


= 2 


X 


j=o 


C K -± 5 1 


2-j 


J=2 


Cx-y> 
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eont-i nuouss &nd t/h* d^ri VAt.1 v*se fcr* cont-inuouB up t-o d 


K-2 


fCxi 


dx 


K-2 


K-1 


Art-*r 'this if ws di f f sTsntl ater ones again d fCxD will not in 


dx 


iC-1 


genoral be continuous. 

Si nee the space 9 *^ vr -a ^ ® mens! on m+K-1 , we may 

fC 1^ y]|f » 1C £• 

chooss^ any eonvcpni f&nl Cm+K-l^ number of basis funeiions. Le*t 

^jCxZ)» j = 1 * S» . . . ►m+K-l be one such basis. Then 

m4-K'-i 

fCx3=2Xj^jC>c> » a::£x:^b 

J=i 


Clearly we can choose the basis function as 

K-l 

C x3 = C >^”“*0 j » a:s;x^^b »J=2»3» . . . 

1 -C m'+'l D 

= x'^ ,a::^x2^b» J = m+1 » m+2» . . . 

This follows directly from the representatl on we gave earlier for 
fCx3. 

The above basis which uses truncated power functions is» 
however* not convenient* especially in numerical computations* 
because of two reasons : 

Ci2> At a point x* fCxD involves in general more than K 
coef f 1 ci ents . 

Cii> For very non-uniform set of knot points <Tr 7 ^>» some of the 
basis functions ^ become nearly linearly dependent on the others. 

J 

This will cause problems in numerical work. 


2.4.2 The B-Spline Basis 

The so cal led B-^llne basis overcomes both the 
objections given above to truncated fsower basis. The need. 
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liil 1 y t is "to choose ba.sis functions which a.re iden’ticA.lly 

2 :ero over a l&rge pari at ihe inierval at approximaiion» a »= ^ 

yi < n =b;in oiher words* ihe need is io choose Jfunciions 

which have limiied suppori over ihe inierval. Thus ihe problem is 

io find an eleir^ni at ^ . ihai is ideniically zero over iwo 

iniervals Ct). » T> 3 and C>7 * Y) bui is noi ideniically zero on 

1 p q 'm+1 

Ctf » Y? 3 . Such a funciion can be represenied as 
P q 

“> K 

. 2 dj 

J=P 

wiih ihe addiiional condiiion 


a X < b 


C2. 43 


q 

2 dj - o 

j=P 


for Y> :S^ X < b 
q 


ca. B3 


Thus* over ihe int rval p < x < q* fCxD has a Kih order polynomial 
represeniaiion and outside this inierval* it is ideniically zero. 
Equating ihe sum of the coefficients of like powers of x io zero 
in ihe expansion of ihe above expression* we get 

q 

J dj yjj = O . i = 0» 1 K-1 C2. ©:> 

J=P 

These equations are homogeneous and have a non - trivial solution 
» 

for d s only if ihe number of equations is less than ihe number 

of unknowTis, t,hat, Is, only if Cq-p5 > 1C, b«oa.use, then the number 
of coefficients d , Cj = p, p+1 , . . . ,ql}, is greater than the 

vJ 

number of equations, which is K. 

If q-p = K, it can be shown tS] that 


p+K 





1 


Cr>i- r,j3 


^ J ^ P»- P"*"! * - . . *p**’IO CS. *73 




Fig. 2.1 B—Spllne of order 2. Fig. 2.2 B“Spline of order 3 



Fig. 2.3 B-Spline of order 4 Fig. 2.4 B—Spline of order 5 



Fig. 2.5 - Figure showing the linearly independent 

spline functions of order 4 over 8 knots. 
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The spline function 


B Cxi) 
P 


p+K 

= H s: ] -vr 


J=p 




» — « < X < « 

C2. e:> 


is €10.1 l&d A B-Splin*' The saubsseripl p denotres that B CxD is non- 

P 

zero only if x is in the interval Cr) , r) , iP • Figures C2.1> to 

p p+K 

C2. 4;> show B-Splines of degree one» two» three and four 

respectively for the case with uniform knot sp^acing. 

The propter ty of limited support of a basis function is 

very useful in computer calculations. Therefore we seek a basis 

K 

^ that is composed of B-splines. The functions B Cx^» 
K,y)f#K 2 p 

Cp = 1 » 2» . . . are linearly i ndependent » IFig. 2.53 and 

are all in S* ^ Now the dimension of the space spanned by 

K,y]f,K““S 

these Cm-K+ID functions is Cm-K+ID while the dimension of ^ _ 

iwy),K— s 

Lfi m+K-1 . Therefore^ another 2K-2 basis functions are required. A 
convenient way of choosing them is to introduce some extra knots 
outside the interval [a»b3. We select these additional knots on 
the real line as follows : 


c, i ^ ^ ^ T), = a 

^ ~ ^ ^m+K+1 ^ ^ ^m+2IC-l 


The original and additional knot points form the extended knot 
set, =!•► 2 m+2K-l!). Though the B-£^lines are 

vJ 

defined with respect to this extended knot set, we will use 
function values only when x e Ca, b3. Thus the total number of 
B-Splines has been made equal to the dimension of „ - and any 

*■.», K.-<3 

fCxD e S’ ^ can be expressed as 

1C, Y) » K 2 
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m+IC-1 
fCx^ * J 
J=1 


j 


X « [ A t b] 


C2. OD 


2.5 A Recurrence Relation For B-Splines 

In computer calcul ations » we need to frequently 
determine the values of B-S^lines» given the value of x. We can 
calculate the value directly from the expression CS. 6D. This 
however is not convenient ^ especially when x is close to 
tlSl. A procedure that is efficient in all cases is descrilbed 
below. We will state a theorem without giving proof £23 
rheoT^em. ~ Let K be an integer > £ and let ^ » J = p^ p+1 » . . , > 
p*K ^ . ... .r ..... ....cu. TK.n .. 

gi ven by 


B 


K-l 


Cx - C > b’^“^Cx> + CC ^ - >0 fcxD 

p p ^ p+K p+1 




ca. 103 


for all real values of x. 

A convenient way of calculating B^Cx2> for a given x is 
to compute the columns of Table £. 1 » in sequence. 



2a 


Table 2.1 



If X e 1^ t,he numbers in 'the first- 

column have the values 

BjCx3 = O for j sr i-1 , J jaf i 


sf ^Cx3 
i —1 




<^i.i - ^i-i" ^ <^1.1- <^i^3 


B® Cx!) 


Cx - 


[ ^ ^i+1 ^i^ ^i+2 ^i^ 3 


In computer CAlculationa* we generally 
use normalized B-splines for the sake of convenience By 

definitionfr normalized B-Splines are those whose sum at a given x 
within the sp>ecified interval is equal to i. The recurrence 
relation for them given by De Cll is as follows 


bJ^Cx:) = 

( D 


( ^i+K “ ^ ) ®i+l 
C ^i+K “ ^i+l) 


ca. 


starting with 




s X a£ 

CX-h^r wise- 
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CHAPTER 3 

B-SPLINES AND T^C METHOD OF LOCAL VARIATIONS 

Interpolation is the process of generating a 
continuous trajectory or curve to pass through specified data 
points. For this process, we need some kind of interpolating 
function. As explained in chapter S, B-^line has attractive 
features for interpolation. This chapter explains interpolation 
using B-Splines. Some numerical examples are given to show the 
advantages of this scheme of interpxalation. In addition a brief 
introduction to the method of local variations for numerical 
solution of optimal control problems is given. H^is method is 
used later in this thesis for solving the optimal trajectory 
planning problem At the end we present arguments to show that 
B-^lines are very <M>ell suited for obtaining numerical solution of 
optimal control problems by the method of local variations . 

3.1 Trajectory Interpolation using B-Splines 

Let » ± = 1 » 2» . . » represent dsita. paints. 

These are the points in the inter polation interval where e 2 <act 
values of the function being approximated are available. These 
may be the result of some observations » for example^ position 
values of a robot Joint at some time instants within the traversal 
time . 

Now consider approximation of a function FCx3 using 
normalized B“-Splines of order K over knots on the interval 



es 

K* %+K-i 3 points . < . . . < 

“ b. Not^«f t/haLt. Irhia^raf is no f’or- Any oJT ■Lh^s* cIaLa point^s to hwer 

knot points though^ of course^ datA and knot points can coincide. 

The function FCxD is given an approximate represent ati on fCxD as a 

linear combination of B-£Iplines over the interval Fx. » x ^ 1 » 

t i m+K-lJ 

that is f 

m+K-l 

FCxD 2 fcc fCxZ> ~ 2 B^Cx!) » x e [^ 1 * C3- 22) 

i=l 

In order to express fCxZ) as a linear combination of m+K-i 
B-Splines we need additional 2K-2 knots as was explained in 
chapters, So» an extended knot set should be formed* with m+SlC~i 
knots. 

Now * the problem is to determine the coefficients a^ * 

i = 1 * 2* . . . so that the value of the function fCx!) can 

be determined at any p>oint within the specified interval. We have 

the exact values of the function at the data pK 2 >ints < x ... * 

X ^ >. The coefficients a. , i = 1 m+K-l , can be 

m+K-l i 

evaluated by equating the summation in eqn. C3. 22> to the exact 
values of the function at the data points , that is , 
m+K-l 

y a, B*^Cx,5 <= fCxp = FCxp » J = 1, . . . . m+K-i C3.3> 

L i i- J J j 

i=l 

Thus we can get m+K^l linear equations which can be written in 
matrix form as 


B A * f 


C3. 4!) 
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where 

T 

- C*1 • • * “m+K-l 3 

f = , . . . , 

In eqn. C3.3:> values of the B-Splines are calculated front eqn. 
C2.112> with resp>ect to the extended knot set ► - • - *^m+21C— 

As ^ at a particular p>oint» only K B'-Splines contribute to the 
interpyolation process* the ?natrix B is a banded sparse matrix. 
Eqn. C3.4!> can be solved for a. For existence of a unique 
solution for a* the condition of Schoenberg ~ Whiteny theorem C23 
should be satisfied : 

— k?ii!ten.y 7'^teorem. - 

Let the numbers »J = 1* 2* . . . * m+K-l> be strictly 

in ascending order. For i = 1* . . . consider B-Splines 

K 

B^Cx2>. Let the p>oints <x^*i=!=i*2*. , . *m+iC~l > also be in 

strict ascending order. Then* for any function values < fCx^D* i 
= 1 » 2* . . . * >* the equations 

m4-K~l 

2 * J = i, a* . . . .m+lC-1 

1=1 

have a unique solution for *1=1*2*. . . * m+K~l > if and 

K 

only if all the numbers < B Cx !!> * J = 1 * 2* . . . * > are 

\j u 

riori-z*r o . 

After finding the coefficients , i = 1, a, . . . , 

Bfi+K-l > we can evaluate the values of the function fCx!) for any x 

in the interval fx^ » x . 3 from eqn. C3.a2). Also we can 

1 m+K— 1 

determine the first CIC-a> derivatives of the function from the 
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f ol 1 owi ng equal! ons 


r 

= i*j.i 

i=l 


C3. 


where 


C ^ X < c 

y y-*i. 


X « 


[ ^1 


-i3 


f^CxD 




m+K-1 
fCxii 
d 

dx'* 


‘J.i 


( J ) ( *j _i , i *J -1 , i -1 ) 

( ^i+K-J “ ) 


C3. 6:5 


CJ+15 < i < Cm+K-ID and 

a^ ^ = a^ , 1 = 1 , 2» . . . , m+K-1 , are the coefficients in the 

representation of function fCx3 in terms of B-S^lines of order K. 


3.2 BouhOARY Constraints 


At this stage we can consider inclusion of end p»oint 
equality constraints in the interpolation scheme. For a Kth order 
B-Spline we can include a maximum of CK-2Z> pairs of end 
constraints on the values of the first CK-2D derivatives of the 
function. From eqn. C3.6Z> it is evident that the coefficients for 
the Jth derivative representation can be expressed in terms of the 
coefficients for Cj-lDth derivative. So* all the end point 





equality constraints can be written in terms or the original 
coefficients^ a^ » i « 1 » and these can be 

included in eqn. C3. 42) itself. 

For i 11 us t rat ion > we consider the case 1C = 4, Here we 
can include a maximum of 4 constraints at the bxipundary points of 
the interval of i nter px:^! ati on. For example » we consider the 
situation where constraints only on the first derivative is 
stipulated. We will illustrate below how this constraint can be 
consi der ed. 


r 

= 2‘i.i 


*^1 » ^-1 ® y-i 


+ a, B 
±^y y 




( ^*1 - 



C <^^*3 - 





Sa^ •the R.H.S. of eqn. C3 72> can be equated to the desired 
constraint and can be included in the matrix eqn. C3.4!> with ^ 

• . - as the elements of^ matrix B Solution of eqn. C3 42? 

gives the coefficient a automatically satisfied 

Similarly for K = 5» we can include a maximum of 
6 constraints at the boundary points. Thus* for instance* 
end-point constraints only on first and second derivatives can be 
i mposed as shown bel ow 

r 

L i ►i i 
l=y'-3 

» > 

Expressing the coefficients a. , s in terms of a s we get the 

jL f X X 


follow! ng expression 
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t C>0 = a 


y-4 


-4 B 




.4 


+ ^y-3 


4 B 


r-s 


4 B 




C C^*2- Cy-si 


* 


[ ^y+± 

4 


4 B 


^-2 


C <:^.2- ^^-2) 


4 B 


r-1 


4 s'* 


V-1 


4 B 


y-1 


4 


4 B 


C V D 


-L ^*“4 i i jL **^“*1, **5^ 


Similarly for the second derivative we 
following equation 


f C x!) = A a B a ^ . a "*■ 4 ^ 

2 j^-4 B ^--“3 2 y~2 2 ^-1 2 y 


wi th 


12 B 


r-^ 


^2 = 


( ^r+i~ C ^y+i“ ^y- 2 ) 


J/+1 ^ j ^- 2J 


C3. 85 


gel Ihe 


C3. 05 
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[ 

^ ^-2 

12 

®2 = 

L 

( C^-3 ] C C^.i- V-a) 

k. 

( ^^+ 2 “ ^^-23 


>-2 

® . 1 
r-i 


C <^.sJ 

( ^^'+ 2 ” ^^-i3 






1 1 
y-i 

^2 " 

[ C ^y+2~ 

C V*1 ' 

( V-i) 


12 

^_1 

1 

r 


C Cy.s- 

C ^^-i3 

C 





12 

r 

B^ 

r-i 


B^ 1 

r 

C ^ja4-3~ ^^-±3 


C ^^^.2" ^,.-i3 


[ ^y 4 - 3 ~ ^;^3 

12 

1 

r 1 

r 




C ^^+ 4 - 3 


C ^^.3 


12 B' 


^2 = 


( 3 C c^3 


Now constraint «rquationa can be written using eqns. C3. 8]) and 
C3.Q2> for the end points and these equations can be incorporated 
in eqn. C3.4!> » as was done earlier for the case K = 4 . 
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3.3 Interpolation Algorithm 


for ih€r 
i 

Stiep 2 

Si^p 3 

£t€?p 4 


S 


St^&p 6 
S^i^'p 7 
St^p 8 

St&p Q 


The not.a.tions used below jire the sanrte as those used 
development of the B— Spline theory . 

Select K. 

Select m-f-i knot p>oints Cy) ^ T) » . . . D > according to 

1 2 m+1 

the desired i nterpol ati on interval . 

Form the extended knot set C<^ » . . . ,C according 

to Schoenberg and Whiteny condition 

Select the range of interpolation ^ ^ satisfying 

Schoenberg-Whi tney conditions t2] together with m+K-l data 
F>oints X. frX- » . . . ►x ^ ^ iTx this interval 

Form the square matrix B of order Cm+K-ID according to 
eqn.C3.32? with resp>eci to the data points selected in Step 
4 and the extended knot set formed in Step 3. 

Obtain the function values fCx.!) j = 1» . . »m4-lC~l 

Solve Lhe equaLion B a = £ Tor e . 

Form -the coef f id enls a.. . . » IC-2 ► 1 = 

J * i 

J+1, . . . m+K-1 > , aceording to eqn CS.ei) 

O&Lermine Lhe funcLion values from eqn. C3.2I) and iLs 

first ClC-25 derivatives from eqn C3. 52) for x e 

t x. . X ] . 

1 m+K-1 


We now consider a few examples to illustrate 
the B-^line interjxilation scheme. 

Example 1 - Find the B-Spline approximations to the functions 2x » 

— O i.X 

and e ' for 1C = 41 within the interval CO, 15.53. 

Solxitton - We select O, 1, 2, 3, 5, 7, S. 11, 13, 14, 15, 16 as 
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the knot j^oints. Here m = 11. The spacings; between knots need not 
be equal. Here, unequal spacing has been selected. Six C= 2K-23 
f'ictitious knots are added to Torm the exterided knot set : 

Extended Knot Set = ^ -0.6, -0.4, -0.2, O, 1, 2, 3, 5, 7, S, 11, 

13, 14, 15, 16, 16.2, 16.4, 16.6 1 




C^_,= -0.2 < x^ = O = rv, 

^m+K-1 = ^ ^m+K-l ^m+1 

The cons.tr aints on initial and Tinal values of first derivative 

are included in the 2rad and 13th equations respectively. So^ the 

first two and last two data points are automatically fixed as O 

and 15.5 respectively. The remaining ECm+K-lZ^ -43 =10 data 

F>oints are selected as the knot points only for convenience. 

the data point set is 


|o, o 


, 1, 2, 3, 5, 7, e, 11, 13, 14, 15. 15.5, 15.5 


} 


Matrix B is now formed according to the above data points. 
Function values or the value of derivative of function tas the 
case may be D at these data p>oints are taken as the input 
Results are shown in Tables 3.1 and 3.2. 

Example 2- Solve the above problem using K = 5. 

Solution - Inputs needed for this case are given below : 

Original Knot Set = /o, 1, 2, 3, 5, 7, O, 11, 13, 14, 15, 16! 


Extended Knot Set 


.(-o 




C)ata Poi nt Set 


= |o. 0 


S, -0.6, -0.4, -0.2, 0, 1, 2, 3, 5, 7, O, 
11, 13, 14, 15, 16, 16.2, 16.4, 16.6, 16 S 
, 0, 1, 2, 3, 5, 7, S, 11, 13, 14, 15.5, 
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15,5. 15.5| 

Here. consLraints on initial and final values of first 
derivative are included in the iSnd and 13th equations respectively 
and those on the initial and final values of £nd derivative are 
included in 3rd and 14th equations respectively. 

Results are qiven in Tables 3.3 and 3.4. 

3 4 The Method Of Local Variations CMLV) 


The method of local variations was originally 
developed by Chymous* Ko [183 and Che^r^noxiS* Ko & Krylov [103 in 
early 60* s. It is a direct numerical method for the solution of 

fixed-time variational problems encountered in optimal control. 
It has the advantage that it yields a feasible subopt imal solution 
at any stage of the Iterative process and it can handle state 
variable magnitude constraints easily. In this method the 
performance index of the problem is minimized by successively 
perturbing an initial feasible state trajectory. The perturbation 
given at each step in the iterative process to state trajectory is 
over a small subinterval of the problem : the trajectory outside 
this interval is not disturbed. By systematic perturbati ons the 
whole trajectory is gradually changed. The perturbations are such 
that the constraints on state and control variables are satisfied 
and. furthermore, the performance index value goes on decreasing. 
The iterative process is stopped when convergence of performance 
index has been achieved. This method finds only a local optimum. 
Ti i . thus, oener ally necessary to start from different initial 
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Table 3.1 


Cubic Spline In'terpd alion Resul'ts 


X 

rC yO =2x 

f*c>o 


O. B 

O. 0998143828 

1 . 0907053941 

O. 0001517023 

4. 5 

8. 9998027288 

2. 0000710248 

-O. 0003650993 

8. 5 

16. 09800264 

2. 0001330314 

O. 0009603793 

10. 5 

20. 9998005010 

2. 0009388950 

-O 0008616201 

14. 5 

28. 09737370263 

1 . 0053686431 

O. 0088074727 


Table 3.2 


Cubic Saline In'Lerpda'Llon Results 


X 

fCx^=:e 

f ‘Cx:> 


2. 6 

0. 7788157051 

-0. 077881103 

O. 0076871747 

4. 5 

0. 6376310678 

-0. 063756501 

0. 0063628581 

8. 5 

O. 4273637035 

-. 0427237594 

0. 0034849449 

10. 5 

O. 3409286649 

-0.0340754188 

0. 0034849440 


14. 5 


O. 2345500467 


-O. 0234948670 


O. 0024130314 











Table 3.3 
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Ouart-ie ^line Interpolation Results 


X 

f C xl) =2x 

f ‘Cxi) 

f ' *CxD 


a 5 

B. 0006631 

2. 00032676 

-0. 0054201 

-0. 0006434 

6. 5 

12. ©QQ6626 

2. 0001S40B 

0. 0004308 

-0. 0003753 

iO B 

20. eososis 

1 . 03308408 

0. 0004808 

O. 0007253 

12. 5 

2B. 00164B4 

2. 00163S4B 

0. 00021 40 

-0.0017643 

14. 5 

2© 000820B 

1 . 30437565 

-O. 0052032 

0. 0174612 


Table 3.4 


Ouartic Saline Interpolation Results 


X 

-s ~ 0 . lx 

rCx3 =e 

r*cx3 

f * 'Cx3 

r* ' 'CxiS 

2. S 

0. 7760663 

-0. 0776780 

0. 00606840 

-0. 0010008 

6. 5 

O. 5222545 

-0. 0525163 

0. 00460011 

0. 0001570 

10. 5 

0. 3501344 

-0. 0353048 

0. 00300574 

1 

0. 0003707 

12. 5 

0. 2863504 

-0. 0283326 

0.00320210 

-0.0013366 

14. 5 

0. 2344050 

-0. 0235463 

0. 00204068 

-0.0002150 
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i-raJ^ct/Ori Tor capt-uring ■the? global minimuift. 

3.4.1 The Optimal Control Problem 


Consider the continuous time controlled system 
described by the set of differential equations 

xct3 = fcxct:?, ^t:>, tD C3.10:) 

where 


t is the independent variable and represents time* 

K ^ is the system state vector* 

u e is the control vector and 

T 

t = J^f^ » f<w» • • »f 3 is a vector function of u* and t 

“ 1 c n — — 

We consider t to be in the interval CO»T]f with the final time 
instant, T, specified. The constraints on state and control 
variables are given by 


j<Ct:> € GCt5 

uCtD e UCt,x:> 


ta. ii:> 


Wl>er e G and U are generally variable closed regions of and 

respecti vel y. 

The optimal control problem Is to find an admissible 
control uCt5 that causes the system C3. 103 to follow an admissible 
trajectory xCt3 and minimizes the pjerformance index 


T 

JCx3 = J gCxCt3, uCt3 , t3 at C3.ia3 

O 

where g is a given scalar function By admissible control and 

state is meant those which satisfy eqn. C3 113 



38 


3 4.2 Elementary Operation 

A procedure called Eleiflenlary Op^era-tiori C1Q3 which is 
basic t,o t^he Met/hod Local Variations is described below. 

Let t* and t** be two time instants sufficiently close 
to each other. Let * be two sufficiently close points in 

state space. Let t^, t^^ be in [0.T3 with t* < t*^ and x" e GCt^!) 
and X* ^ e GCt^^D. A supplementary variational problem is now 
stated. 

SuppiementcLTy KoLriat toncl Problem. : 

Determine the control xiCt!)» t e [t*^t^M which takes 
the system of eqn. <13. lOD from the initial state x* to the final 
state * minimizing the performance index 

t 

AJ = J gCxt u, dt C3. 133 

while satisfying constraints stipulated by eqn C3.113. 

By an elementary operation is meant an operation of 
solving the supplementary variational problem II S3 Thus» the 
elementary opHSrration basically consists of 

Ca3 deter mining whether a control exists which takes the system 
from x‘ at t‘ to x* * at t* * ► while satisfying the constraints of 
eqn. C3,113 and 

Cb3if there exists such a control, then, to find the control which 
renders the performance index C3. 133 minimum. 

For use of elementary operation in MLV, it is not 
necessary that the minimization stated above be done exactly — 
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■approximate solution of the elementary operation considerably 
simplifies MLV in many situations. 

34-.3 The MLV Procedure 

MLV repeatedly executes the elementary operation to 
solve the optimal control problem defined in sec. 3. 4 1 . The time 
Interval over which the optimization is to be done is divided into 
N subi nter val s > not necessarily of equal length^ given by O < t^ < 
t^ < . < t^ = T» with N sufficiently large. 

An initial feasible state trajectory x^CtZ? is chosen. 
This> in general ^ is not a trivial task and special search 
procedures may have to be developed to generate a feasible 
trajectory. Now* elementary operation is applied over the 

Interval tO»t^3. This opperation will not disturb the state 
trajectory outside this interval^ but will change it over C0,t^3 
to give an overall performance index value over the interval CO»T3 
lower than its value before the elementary operation. Next the 
elementary operation is done over Ct »t ]» etc » till the whole 

i idi 

inter va.1 lO.T] is covered. This completes one iteration of MLV. 
We repeat the whole process as nwany times as required to converge 
to a minimum value of performance index. 

It should be noted that MLV in general captures only a 
local minimum. 

Chernous*Ko and others [17*20^21,223 have discussed 
ways of organizing the details in the MLV procedures, especially 
in the context of discrete time problem formulation. 
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The main advantages of MLV are that 

Ci:> it does not require large computer storage. 

CiiD we get a feasible suboptimal state trajectory anywhere we 
stop the Iterative process. 

CiiiMt is easy to handle state variable magnitude constraints 
and 

Civ^ in many situations the elementary operation turns out to be 
computationally simple and fast 


34 -. 4 - Suitability Of B-Splines For MLV 


If MLV is used for j j x 

trajectories interpolated by 

B~Splines, we get the f ol 1 owi ng advantages ; 

C15 For ro.st.hllshmont of Lho corrUnuou^ trajectory the 

IntorpoLtiorr =ch.m. rr^lrrg B-Spllr..= i. .dv.nt.goous, Thi. 

helps us to use the more accurai o j ±j 

ui at,e pp approximation of system 

rather than that based on finite differences 

CiiD If we use B-Splines for the interpolation of state trajectory 
we can get the cor.tinuous time approximation of state derivatives 
and control functions also. These trajectories can now be 
discretized by simply dividir^g the total traversal time into N 
subintervals. So. we can avoid the assumption of constant input 
between two consecutive time instants as is done in MLV applied to 
the discrete time, i . e. , the finite difference, formulation of 
the optimal control problem. Ir, fact, a good handle is now 
available for continuous information of the control functions with 
very good accuracy. 



41 


Cili:) In MLV* the B-Spllne interpolated state trajectories are 
perturbed indirectly by the variation of interpol ati on 
coefficients • This has a great advantage because of the 

limited support of B-Splines Thus ► unlike in the finite 

difference method ^ if one interpol ation coefficient is varied^ the 
state and control will be affected only over a certain 
subinterval. This is» indeed, what is basically wanted in MLV 
B-'Spline approximation is thus ideally suited for MLV and 
computations become simpler. 

The above features are important in robot 
path-planning involving obstacles. Suppose the robot arm meets an 
obstacle at some specific time instant within the traversal time. 
Then the robot arm trajectory can be modified in a neighbourhood 
of that instant only by perturbing the involved coefficierit in 
order that the obstacle may be avoided. It is not needed to 
recompute the whole trajectory. 

34.2 MLV Algorithm With B-Spl^^ies 


The following notations are used : 

K = Order of the B-Spline 

4 kith B-Spline coefficient for Jth derivative of the ith 

stfiiicf variable. J = O, 1 K-2 ; i = 1 , . . . , n ; 

kl = 1 ► . . . fm+lC-l. a^ . , . represents coefficients for the ith 

state trajectory. 

At represents discrete time instant t^ and satisfies 0< < N 

J[13 = Performance index associated with the whole trajectory over 





C0»T3 the 1th iteration. 


P = Saturation 

1 ndi cator . 

It actually measures the number 

of 

successf ul 

local 

var iations 

p = 

0 i mpl i es 

that none of 

the 

vari atlons 

is valid in the 

current 

iter ati on 

and saturation 

i s 


reached. 

Delta == Amount of perturbati on in a particular B-S^line 
coefficient in the state trajectory representation 

E>el_min = Minimum permissible perturbation in a B-Spline 
coefficient in the state trajectory representation. 

2[ k] = Performance index associated with the hth subinterval 


Ct >t 3 = Interval affected by a particular B-^Spline coefficient. 

Cf e* 

Pli = Performance index associated with the interval Lt, ^t 3 

b e 

bef or e pwer tur batl on. 

PI2 = Performance index associated with the interval tt^»t 3 after 

b e 


per tur bat i on. 


The MLV algorithm using B-2pline representation is as 


follows : 


Step 1 : Obtain a B-Spllne approximation of feasible state 
trajectories and their K~2 derivatives and hence obtain the 
coefficients j i J - »K‘-’2 ; i = ; 

kl =1, . . . 

Discretize the above trajectories at instants t^, k = 
O, . . . ,N. Calculate 
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C&lcula'te Lhe- initial pHSrrrormanc«& index by 

N 

Jt03 = '2 
M=1 


Step 2 
Step 3 
Step 4, 
Step 5 


Step 6 
Step 7 
Step e 


Set, 1=1 and set val ues of Del ta , C 
Set P = O 
Set i = 1 

Set kl = C , where a. , a. , . . . , a^ are the 

IM 13 C 

IN- i 

coefficients contributing to the function representation 
at the initial F»oint. 

II>etermine the interval [t, ,t 3 affected by a. , 

Set a = 1 
Cal cul ate 


Pli 


A:=b 


Stifi'p Q : C>^&1 "La = 

j 14 **“ Ctelta 

i » kl 1 » kl 

Step 10: Calculate the ith state* its derivatives and associated 

controls in the interval [t^*t ] with the new values of 

b e 

Ca , . * J = O* . . . * K-S] and check whether 

j * i * k 1 

Cil) the new states are admissible 

CiilJthe control inputs required to translate the states 

in the interval Ct^,t 3 along the changed trajectory are 

b e 

admiasibl e. 


If *Yes* then go to Step 11 else go to Step 12. 


Step 11: Calculate 


' I 2”'' 

k-h 


PI 2 
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ir PI2 PIl » then go to Step 12 else go to Step 13. 

Step 12: Restore the coerficients ; 

j = a. , , - Oelta 

0*i»kl 0»i»kl 


Set. a = —sl 

If a = -i t,hen go t.o Step 0 else go to Step 14 
Step 13- Success met 


Step 14- 


Step IS: 
Step 16: 
Step 17: 


j[i] = j[i-i] - Pii + Pia 
p = p + 1 
kl = kl •♦•I , 

if kl < C , then go to Step 6. where & , . . . 

TU-H 

’ '^m-fK-l the coefficients contributing to the function 

representation at the final point. 
l=i'+l ; ifiSn then go to Step B 
1 =1+1 ; ifPj^O then go to Step 3 

Set E>elta = Delta/C 

If Delta > E>el_min then go to Step 3 


Step IS: Stop. 

Different values of Etelta can be chosen for 
coefficients for representation of different state variables. 
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CHAPTER 4- 

MINIMUM ENERGY TRAJECTORY PLANNING 

In this chapter an off-line minimum energy path 
planning scheme has been discussed This scheme consists 

essentially of two stages • formulation of initial feasible joint 
trajectories using B— Splines and optimization of these 
trajectories for minimum energy using MLV. 

A feasible EE trajectory in cartesian space is 
initially chosen to move the EE from the specified initial point 
to the final point in a fixed time. Then the corresponding 
feasible joint trajectories are obtained using the inverse 
kinematic algorithm Feasibility here implies that the 

constraints on joint position, velocity, acceleration and 
joint -motor torque are not violated 

The second step is the optimization of the above 
trajectories for minimum energy using MLV. The aim of this 

optimization problem is to find a path for the EE movement between 
the two specified points, by locally pserturbing the initial 
feasible joint trajectories so that the total input electrical 
energy lost as losses in the joint motors is minimized. Here, the 
geometric path is not sprecifled apriori. Only the initial and 
final configurations are specified. So, geometric path planning 
and trajectory planning are done simultaneously. 


4-.1 Dynamic Model Of The Robot Manipulator 



For computer simuia’ti on of the robot arm motion it is 
necessary to have a dynamic model of the manipulator » which is 
actually a set of mathematical equations depicting the dynamic 
behaviour of the manipulator^ that iS| the relationship between 


the Joint 

torque 

and 

joi nt 

conf i gur ati on » 

velocity 

and 

accel er ati on. 

There 

are 

mai nl y 

two conventional 

appr caches 

f or 

obtaining the d>nami 

c model, namely, Lagrange -Euler CL. E. 

and 


Newton-Euler CN.E. D formulations. 


bel ow 


The L. E. formulation for the robot dynami cs is given 


rct:) = DCqct:):)qct:) + hCqctD^ qct:>:) GCqct::>:> CA ±:> 

wher e 


TCt:> = an nxl torque vector at Joints i = 1 » . >n 

= CT.ct:>, . . . ,T 

IS n 

qCt2> = an nxl vector of the joint px^sition variables of the 
robot arm 

= tq, . . . . ,q 

1 n 

qCt5 - an nxl vector of the joint velocity of the robot arm 

= [q^ Ct:> * . . . ,q CtD 

1 n 

qCtD = an nxl acceleration vector of the robot joints 

= Cq,Ct5. . . . »q 

1 n 

IX qi = an nxn symmetric inertia matrix 
hCqt q5 = an nxl nonlinear cor i oils and centrifugal force 
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The inverse dynamics problem CIDA!> can be solved using eqn. C4.i2>* 
that is^ given the instantaneous Joint position^ velocity and 
accel er ati on » obtaining the required actuator torque. The 

expansion for the elements of D* h. and G are given in Appendix A 
Although computational effort required in calculating 
the dynamic coefficients in L. E. formulation is quite large» it is 
still preferable in off-line optimal path planning scheme because 
of its differential equation form which is very convenient for 
obtaining a state space model of the robot arm motion. State 

space model can be obtained by defining the state variables and 
inputs as follows * 



= ‘’a 


U = T 
S 2 


C4 SZ) 
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n 


n n 


“i - 2°i.k 
k=i 


2 2 "i.j.k ■'j 

J=1 k=l 


C4.. 4D 


i = 1» 


. .n 


With the above formulation » Joint acceleration vector is given as 

,T 


5 = tqj. . . .q^)^ = tXj. 


•"‘n’ 


C4, s:> 

Hi£?r«Ep» it is not noce-saary to write the state eqn. (14. 42) 
in the conventional form k = fCx>u>t!> because in using MLV» given 
the system state trajectory between two points ► we will need to 
find the inputs which will cause this state transition^ and the 
form of eqn.C4.42) enables us to determine this directly. 


4-.2 METP Problem Aho Its Solution 


4-.e.1 Problem Formulation 


Having the state space model of the robot manipulator 
optimal control problem is formulated as a fixed time, two point 
boundary value problem. The states C Joint positions and 

velocities:> are assumed to be specified at initial Ct = and 

final Ct = TO points. Thus, 

O i = 1 , 2, . . ,n C4.. ea:> 


x^co:> 


x^CTO 


x^C03 - x^Q 


x^CT) = x^.j. 


i = n+1 , . . . ,2n 


C4, 6b:> 


Eqn. C4.6a> stipulates that the initial and final velocities of 
the robot Joints be zero. Eqn. Cl4.SbI) stipulates the initial and 
final robot Joint positions. Constraints on the magnitude of 


4Q 


and their derivatives are also specified at all t « C0^T3. 
Input constraints, that is, constraints on magnitude of actuator 
torques are also given. These bounds are also specified as 
constant bounds of the form . 


X, , < X < 

i min i 


, i = n+1 , 


i max 

This specifies bounds on Joint p>ositions . 


, 2n 


X, . < X < X, 

i mi n i i max 


n 


X, , < X < X, 

i mi n i i max 


X, , < X < X^ 

i mi n i i max 


U, . < u < U 

i mi n i i max 


► n 


C4 7aD 


C4 7b:) 


i = 1, s, . . 

This specifies bounds on robot joint velocities . 

i‘*^i,4£!, . . . ,n 

Thiis specifies bounds on robot joint accel er ati ons . 

i = 1 , iS , . . . , n 

This specifies bounds on robot joint jerks . 

» i = 1 , 

This specifies bounds on motor torques . 

The performance index for the problem is defined as 

f ol 1 ows : 


C4. 7c:) 


C4. 7d:> 


C4. 7e:) 


T 

n 

T^Ct:5 j dt C4.85 

1 

where 

T = total time of traversal 

T^CtD = Torque for the ith Joint of the manipulator 
Selection of this, pserformance index is motivated by the fact that 
the servomechanism of each robot Joint gerierally uses permanent 
magnet dc servo motors which are armature controlled So, the 
torque developed by each Joint is proportional to the armature 

iS 

curra^n't as flux is constant* JSo, is pr opx>r tional to the coppMSfr 




BO 


loess in it,h Joint motor a.nd / t dt ia proportional to th» 

O ^ 

energy lost in the ith Joint motor as copp»er loss in the interval 
[O, T3 . So, minimization of this performance index results in the 
minimization of electrical energy lost in the motors, assuming 
that the other losses in the motors are constant . 

Now we will state the METP problem as follows : 

Problem. StaLt^^rru&rLt-- Find opLirna.! eontrol u and op-tlm*,! Btat,* 

iraJ^pct/Ory k such Ihat. th«& p^erformanc^ index given by eqn C4.82> 
is minimized -together with satisfaction of the end constraints 
given by eqn, C4,62) and constraints on states and inputs given by 
eqns, C4. 72) . 

4-.2.2 Solution To METP Problem 

MLV algorithm presented in chapter 3 is used to solve 
the METP problem stated above. 

First of all an initial feasible cartesian path is 
chosen from the initial point to the final F>oint. In order to 
facilitate choice of a feasible trajectory, the time of traversal 
is chosen longer than dictated by minimum traversal time 
consideration so that the effects of corlolis, centrifugal forces 
etc are small and all the constraints are satisfied without much 
difficulty. Some selected on this cartesian path and 

corresponding Joint conf igurations are obtained using IKA. Then 
B-Spllnes are used to fit these points and the corresponding Joint 
trajectories are obtained. Trajectories for Joint velocities, 
accelerations etc are also obtained using the spline coefficients 
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for the derivatives. Now, all the steps of the MLV algorithm are 
followed starting with a suitable value of Delta In Step 10 
constraints are taken as those specified in eqns C4 and C4r.72)- 
The algorithm is applied for more than one starting feasible 
trajectory and different perturbation values are also used. 

4.3 Results Of Computer Simulation 

The MLV algorithm for the solution of METP problem was 
verified through the computer simulation of first three Joints of 
PUMA 550 robot. Two examples are considered. 

4-.31 METP Using Cubic B-Splines 

Paroblem. 1- Plan the optimal robot Joint trajectories and 
cor r espondi ng time sequences of Joint velocities and acceler ations 
to move the EE from CO.SSmt O. 213» -“O. OQ2!) to C— O. 405» -“0.i04» 

-0.041) in cartesian coordinates in a fixed time. Find the 
required actuator torques also as functions of time. 

Solution - As only first three Joints of PUMA 560 are consider ed» 
it is not needed to specify the orientation of the EE> which is at 
the third Joint. 

For the selection of the feasible trajectory a time of 
traversal of 15.5 secs was found to be sufficient 10 knots were 
selected in between and the EE configuration at these knot points 
was converted into corresponding Joint conf i gur ati ons using IKA 
C Table 4.15. The initial and final velocities of all the robot 

. -rary 














S4 


Joints were assumed to be zero This is because it is assumed 
that the robot arm starts at rest and comes to rest at the end oT 
the trajectory. Now> B~Splines were used to fit these knots. 
Selected knot set and extended knot set were the same as those 
selected in Example 1 in chapter 3. So» the same spline matrix 
was used because only the vector f in eqn C3. 42) was changed. 
Matrix B remained unchanged as it depends only on the knot sets 
and data points. This is another advantage of B-'Eplir^es 

The bounds on the Joint position* velocity and 
acceleration are shown in Table 4.3. Torque bounds are also shown 
in Table 4.3. The time of traversal was divided into 300 equal 
intervals for the purpose of calculating the performance index 

using the trapezoidal rule. The initial value of the performance 
was found to be 3663Q units. 

Now a value of 30 was used for D&lt& and 1 for 

E>el_min MLV was applied to perturb the nominal trajectory and 
after 37 iteration's the value of performance index was found to be 
14463 units. Tlie initial and optimal trajectories for joint 
positions* velocities and accelerations are shown in Figs. C4.i2) 
to C4.02). Cor r esporidi ng optimal actuator torques are shown in 
Figs. C4 102) to C4 132). Variations in p^erformance index are shown 
in Fig. C4.132) against iteration number. 

Again* another nominal trajectory was selected and 

perturbed* starting with a value of Delta as 15. The performance 
index was reduced from an initial value of 1Q361 units to the 

final value of 18033 units in 15 iterations 
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Fig 4.2 - VELOCITY PLOT OF JOINT 1 
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Fig 4.8 - VELOCITY PLOT OF JOINT 3 
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UE PLOT OF JOINT 3 
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+.4 CONCLUSIONS 

From Lhe rersulLs obtained above, It is evident that 
the METP algorithm finds a local minimum and it depends on the 
starting trajectory and perturbation values. In order to capture 
the absolute minimum we must try different initial trajectories 
and different perturbation values. 

Also, it is clear that the p>erformance index reduces 
very fast in first few iterations and after that its convergence 
is sluggish. So, we can stop after few iterations with a 
significant reduction in p>erformance index. 



73 


CHAPTER 5 

MINIMUM TIME TRAJECTORY PLANNING 

In the present Industrial environment, mani pul ators 
are required to be operated at high speed, that is, total time of 
manipulator movement needs to be minimized to perform a particular 
task. Minimum Time operation is specially important in the case 
where robot is required to perform some repetitive Job. 
Conventional trajectory planning schemes generally deal with MTTP 
only. The usual approach is to first generate a non-optimal 
geometric path and then minimize the traversal time of the EE 
movement along this path. Thus, traditionally, geometric path is 
not changed during optimization . 

In this chapter the solution to MTTP problem has been 
obtained by using MLV. Ari algorithm has been developed and applied 
to spline interpolated trajectory to find a time optimal path from 
an origin to a destination . This algorithm is based on 
successively shrinking the whole knot sequence of the 
Interpjolation interval and applying MLV to the spline coefficients 
to maintain the feasibility of the Joint trajectories. 

5.1 Problem Formulation 

The problem formulation assumes the state space model 
given by eqn. C4. 4!) for the dynamics of the robot arm motion . 
This is reproduced here for convenience . 
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n n n 

' 2 "^i.k ^ * 2 2 "i.j.k “j \ 

k=l J=i k=l 


G. 


CB. 13 


i = 1> . . . ,r. 


Joint, pos,it,ions and velocit,iers are* assumed t,o be sp^eciTied at, t,he 
Initial Ct = 03 and final Ct = T3 time instants , that is , 

x^C03=x^CT3=0, i=l. . . ,n C5. 2a3 


i = n+1 , . . . ,2n C5. 2b3 


x^CT3 = X 

where ^ 

th& constraints on 
acceleration and Jerk2> 
as constant bounds : 


iT 

n-^l » . . , ^2n are specified values Also 

state variables Cposition^ velocity^ 
and those on control Inputs are specified 


^i 

ml n 

< 

^i 

< 

^i 

max 


i 


n+1 

tr 

k . *2n 

C5. 3a3 

^i 

min 

< 

^i 

< 

^i 

max 

» 

1 

= 

1 . 

2 k. 

. . *n 

C5. 3b3 


min 


^i 


^i 

max 

» 

1 

= 

1 » 

2. - 

. . *n 

C5. 3c3 

^i 

min 

< 

^i 

< 

^i 

max 

f 

i 


1 . 

2, . 

. . *n 

CB. 3d3 





^i 

min 

< u 

'i 

< 

^1 

max 


CB. 3e3 


It is desired to find the optimal path> Ctime sequence 


of p^osition* velocity* acceleration etc.Di. and desired control 
inputs so that the final time T is minimized. The HTTP problem can 
now be stated as follows : 

Pr'ohl^m. St<at«nwnt — Find the? optimal state x Ct3 and th* 

it 

corresponding optimal control input u Ct3 satisfying eqn.CB. 13 so 
that the time of traversal is minimized, with the end point 
constraints Ceqn. C5. 233 and state and input constraints Ceqn. 


C5. 333 satisfied. 
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5.2 HTTP Algorithm 

The following notations are used : 

K = Order of the B— Spline 

*J,i,kl “ B-Spline coefficient for jth derivative of the ith 

state variable . J = O, 1 , . . . ,K-2 ; i = 1* . . . »n 

kl = 1 , . . . ,m+K— 1. a^ ^ represents coefficients 

for the ith state trajectory . 

P = Saturation indicator for a particular value of the variable 
labelled Delta . It indicates the number of successful local 
variations for a particular E>elta . P = O indicates that there 
have not been any successful local variations and Implies that 
the current value of delta should be changed . 

^ ~ Saturation indicator for the whole range of Delta. r = O 
indicates that none of the variations is valid in the current 
iteration and saturation is reached ^ that is. final time T has 
reached the minimum value and no further reduction in it is 
possible. 

Delta = amount of perturbation in a particular B-Spline 

coefficient in the state trajectory representation. 

Del -min = minimum permissible perturbation in a B-Spline 

coefficient in the state trajectory representation. 

CCk^ = A particular knot point of the eictended ktiot set » 
k = 1 , S , . . . » m+2IC-l 

Ctj^jt^] = Interval affected by a particular B-Spline coefficient 
The HTTP algorithm using B-Spline representation is as 



f ol 1 OWS : 
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Sti^p 1 : Ctstain a B“Spline approximation of feasible state 

trajectories and their CIC-"i22> derivatives with respect to 
the extended knot set ;k=l>. . . 

where 


Store the 


spl 1 ne 


— total number of original knot points 

coefficients a. . , . . j = , 

J f 1 I, K X 


IC-2 


• - • ; kl = 1, . . . , m+K-1 . 

S i C’p 2 : S&t, r = O 

Step 3 : For k =K+1 tom+ 2K - 1 do 


i 


C[k3 = CtkJ - AZ . f 

where 


1 » 


A2 = Ctk3 - 
t = constartt < 1 

Set T = CCm+K] 

Step ^ : Set 1 =» 1 , Delt&i C 
Step B ; Set P = O 
Step 6 : Set 1=1 

Step 7 : Set kl = e , where a , a » . . . ,a are the 

ZINI X c C X 

IN 

eoemdent* eontri butirjg to the ruriotion representation 


Step 8 : 
Step Q : 
Step 10: 

Step 11 : 


at the Initial point. 
Cteterirdne the interval tt 
Set a = 1 
Del ta = a»E>el ta 
a = a + Delta 

Calculate the 1th state > 
controls in the interval 


. , t ] affected by , , . 

be 0,1 ,kl 


its derivatives and associated 

[t^. t ] with the new values of 
b e 
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a., , , ^ ► £J = 0^ . . ^K-21 and check whether 

j » i » kl 

Clothe new states are admissible and 

C ill) the control Inputs required to translate the states 
in the interval ttj^»t^3 along the changed trajectory are 
admissible 


If ^Yes* then go to Step 13 else go to Step 13. 

Step 12: Restore the coefficients 

fit. - , . . I I j Oel ta 

0» i > kl i » ki 

Set a = —a 

If a = — i then go to Step lO else go to Step 14. 

Step 13: Success met 

P = P + 1 


Step 14: ki = kl +1 

If kl SC , then go to Step 8» where a » 

C-i . 

rN+l 

tO. . are the eoef ficients cont,r i buli ng 1,0 t/he function 

m+K-l “ 

representation at the final point 
Step 15: i = i + 1 ; if i £ n then go to Step 7. 

Step 16: 1=1+1 ; r=r+P; 

if P jzf O then go to Step 5. 

Step 17: Set E)elta = Delta/C 

If Delta > Del_min then go to Step 2. 

Step IS: If r 5 f O then go to Step 2. 

Step IQ: Stop. 


5.3 Solution To HTTP Problem 


Some important steps of the above algorithn must be 
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clear for the solution of HTTP problem. 

Here for the inter polation of the state trajectories 
using B-Splines dummy knots are selected as follows: 


Cl = Ca = . . . = C^_1 = O 

This is done purposely because, otherwise, during the iteration 
process, shrinking of the knot sequence may result in violation of 
the state constraints at the initial and final p>oints. By making 
the dummy knots same as the end knots, shrinking of the knots does 
not violate the end point constraints. 

After selecting a suitable interpolation interval and 
a Cpreferably^ feasible trajectory on it. Joint trajectories are 
subjected to MLV algorithm Knots are shifted by equal percentage 
of their separation and then spline coefficients are varied to 
cheek all the constraints. 


5.4- Computer Simulation Results 

The above algorithm has been verified by computer 
simulation of first three Joints of Pl’MA 560 robot manipulator. A 
simulation example is given below. 

Example 1 - An initial time of traversal of 27.5 seconds was 
chosen to move the EE from a point CO. 253m, 0. 35Qm, — 0. IBQrD to 

the final point C-0. 048m, 0.456m, 0.0 SmD in cartesian space. The 
original knot set and extended knot set were selected as follows — 
Original Knot Set - O, 2.5, 5, 7.5, lO, 12.5, 15, 17.5, 20, 


22.5 , 25 , 27.5. 



Table 5.1 
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Knot sequence and corresponding initial 
data sequence Tor Example 1 


Joint 

KnotC sec 2) 

1st 

— 

2nd 

Cdegr ees> 

3rd 

0 

35 

20 

oo 

2. 5 

45 

25 

80 

S. O 

70 

15 

lOO 

7. 5 

90 

-5 

150 

10. 0 

100 

30 

ISO 

12. 5 

SO 

-SO 

190 

17. 5 

75 

-110 

170 

o 

d 

lO 

-1 40 

140 

22. 5 

-30 

-150 

130 

iSB* 0 

-70 

-160 

lOO 

27. 5 

-65 

-170 

95 
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Extended Knot Set - 0. O, O, O, 0, 8.5, 5, 7.5» 10, IS. 5, 15, 

17.5, SO, SS.5. S5, S7.5, S7.5, S7.5, S7.5, 
S7. 5. 

Ete.ta Point Set - O, O, O, S. 5, 5, 7.5, 10, IS. 5, 17.5, SO, SS.5, 

35 , 37.5 , 37.5 , 37.5. 

and final velocity constraints have been included in Snd 

l4th equations respectively. Initial and final acceleration 

'^cjHstra.i nts have been included in 3rd and i3th equations 

^'^specti vel y. Initial Joint values, at the data points, obtained 

inverse kinematic analysis are shown in Table 5.1. 

Continuous Joint trajectories are obtained by fitting these data 

Fxiints and MTTP algorithm has been applied. Delta was chosen as 
30 

And reduee-d by a factor of 1.5 at each step. Del_miri was 
^^lected as 1. Minimum time obtained was 34.376 seconds. The 
^ihal extended knot sequence was 

0, o, o, O, 3.316, 4.433, 6.648, 8.864, 11.08, 13.3S6. 15 513, 

^■^•738, 10.044, 33.16, 34.376, 34.376, 34.376, 34.376, 34. 376| 

optimal trajectories for Joint positions and velocities are 
Shown in Figs. 5.1 to 5.6. Torque plots are also given in Figs. 
7 to B. e. 

Conclusions 

From the torque curves, it is evident that the above 
"‘'^thod gives a subopt imal solution. The actual minimum time 
uti on depends on the separation between the knots also. 


In 
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ordfifr trO minimize -time some of 'the knot spacings may need -to be 
increased [13], Here equal knot spacings have been considered. 
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CHAPTER 6 

CONCLUSIONS 

In this thesis two techniques of off-line optimal path 
planning have been presented Some of the salient features and 
limitations too are given below . Some suggestions for future 
scop>e in this area are also given . 

61 Salient Features And Limitations 

Cl!) A combination of B-Splines and MLV has been used for the 
optimal path planning of robot Joint trajectories . The important 
feature of this is that MLV Itself is efficient and simple to use 
, and use of B-Splines makes it even simpler and computationally 
more efficient . 

C25 Saline interpolation for Joint trajectories gives good 
approximation to the actual trajectory . Also * it ensures the 
continuity of the derivatives of the function to be Interpolated ; 
which is desirable in robot path planning problems for meeting the 
constraints on them . 

In the Minimum Energy problem the total energy lost in the 
Joint drive circuit has been minimized and hence the total 
electrical energy consumption . This is of significance in the 
case of robots which have to perform repetitive tasks . Most 
assembly like robots would fall into this category . Since motor 



rating is dependent on temperature rise consideration and since 
temperature rise is dependent on the losses in the motor , this 
optimization enables proper choice or motor size . Alternately » 
for a given motor choice , the motors would be operated in the 
most efriclent way to perform the stipulated task 

C4I> Minimum Time control , that is , selection of a time optimal 
path between two specified EE positions , has been presented as a 
separate problem . 

The procedures used for determination of optimal 
trajectories and the corresponding motor torques for the rAlnimum 
energy and minimum time problems have been shown to be simple and 
computationally efficient . 

CB!) Dynamic and jerk constraints have been taken into account in 
both the problems , which is not the usual feature in most of the 
works done earlier . 

C6!) Both the methods can tackle obstacle constraints without any 
fundamental modification to the main algorithms . 

If splines of high order are used , the simplicity of 
B-Spline approximation is reduced . The bandwidth of the banded 
matrix involved increases and the piecewise polynomial 
approximation tends towards becoming a pure polynomial 
approximation . High order B-Splines are not attractive from the 
view point of MLV also since the interval of local support of the 
splines increases as their order is increased . A Judicious choice 
of spline order » is therefore necessary . We have found spline 
order of B to be the best for robot path planning using MLV . 
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6.2 Suggestions For Further Work 

This work can be extended for path planning problems 
including obstacle constraints . If a good way of representing 3D 
objects in Joint space is available , this method will prove to be 
efficient for collision -free path planning problems. 



APPENDIX A 


CALCULATION OF DYNAMICAL COEFFICIENTS 1 111 


The; dyn&nd. ca.1 co6l'f'lcierit>s , 'Lh&t. Is » t-Ke elemeri'Ls cif 
the matrix D , H and that of the vector 6 ,for the Lagrange Euler 
formulation of the first three Joints of PUMA 560 Ceqn 4. 4!> are 
calculated as follows. 


Let = sinCq^^ 
= cosCq^3 

where 
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g = O S062 m/sec 


Pseudo inertia matrix for ith joint is 
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Now the coerficients are calculated as 
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APPENDIX B 

LINK PARAMETERS AND MASS PROPERTIES OF PUMA 560 ROBOT 

The link parameters associated with the first three 
Joints of the PUMA 560 robot arm are given below . 


Joint No. 

CiD 

“i 

C degrees!) 

*i 

C meters!) 

C metersD 

1 

-OO 

0 

0 

2 

0 

0. 432 

0. 14Q5 

3 

OO 

O 

0 


The mass properties are specified as follows : 

» the inertia matrix for ith joint is given below . 


I 


i 



XX 


O 

I, 

i yy 

0 



diag. = tO. 0071* 0.0267 * 0.02673 Kg-meter^ 
diag. Ig = to. lOOO* 0.7300 , 0.80253 Kg-meter^ 
diag. = [0.0222 , 0.2160, 0.22453 Kg-met*r^ 

= I ^ th& position vector of the centre of mass 

of link i with resp>ect to the ith coordinate system . 

S, = CO.O, O. O , 0,0733^ meter 
= C -0.432 , 0.0 , 0.03^ meter 
= CO.OO, 0.00, O. lOl"^ meter 

m is the mass of ith Joint 

i • 


1 
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